package routingAlgorithms;

import java.util.ArrayList;
import java.util.HashMap;

import messaging.MessagingServices;
import simulation.SimulatedGridReading;
import utilities.Constants;
import utilities.Utilities;
import animation.AnimatedMap;
import dataobjects.Direction;
import dataobjects.Edge;
import dataobjects.Position;
import dataobjects.RoutingOption;
import dataobjects.Seeker;
import exceptions.DestroyedException;
import exceptions.ImpossibleException;

public class RoutingAlgorithm {

	//FIXME: Handle "seeker personal bubble" - seekers can pass through each other and the HVA
	//FIXME: Change interface to be event driven by seeker asynchrnous updates
	//TODO: allow animation to be turned on and off for visual debugging and monte carlo

	private ArrayList<Edge> obstacleList = new ArrayList<Edge>();
	private ArrayList<Edge> visualIedList = new ArrayList<Edge>();
	private HashMap<Edge, Integer> sonarMap= new HashMap<Edge, Integer>();
	private ArrayList<Edge> safeList = new ArrayList<Edge>();

	private ArrayList<Seeker> seekers = new ArrayList<Seeker>();

	private boolean goalFound = false;
	private int seekerSteps = 0;
	private int hvaSteps = 0;
	private int seeker1Steps = 0; //this is used to determine when seeker 3 should start moving
	private int endPosition = 9;

	private MessagingServices ms;

	private Seeker hva;
	private int advanceCount = 0;

	private static final int numEdges = 2 * Constants.GRID_WIDTH * Constants.GRID_HEIGHT 
	+ Constants.GRID_WIDTH + Constants.GRID_HEIGHT;
	private static final int maxSearchDepth = numEdges/2 + 1;

	SimulatedGridReading simulatedGrid; //Only for simulation use
	AnimatedMap animatedMap;
	boolean started;
	boolean suicide;
	Seeker leader;

	public RoutingAlgorithm(boolean simulation, boolean suicide){
		animatedMap = new AnimatedMap(this, simulation);
		animatedMap.draw();
		this.suicide = suicide;

		if (!simulation){
			initSeekers();
			ms = new MessagingServices(this);

			Thread t = new Thread(new Runnable(){
				public void run(){
					ms.run();
				}
			});
			t.start();

			//command seeker 1 to go to 0, 0
			ms.sendMessage(0, 0, 0);
		}
	}

	/**
	 * Depth First Search, the standard search algorithm for seekers.
	 * @param pos
	 * @param depth
	 * @param explored
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> dfs(Position pos, int depth, HashMap<Position, Integer> explored) throws ImpossibleException {//TODO, what is explored? can it be global?
		if (pos.getX() == Constants.GRID_WIDTH && pos.getY() == Constants.GRID_HEIGHT){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		if (depth == 0){
			return null;
		} 
		if (explored.containsKey(pos) && explored.get(pos) >= depth){
			return null;
		}
		explored.put(pos, depth);
		ArrayList<Direction> branches = Utilities.getDfsBranches(pos.getDir());
		for (Direction dir : branches){
			int x1 = pos.getX() + dir.getHoriz();
			int y1 = pos.getY() + dir.getVert();
			Position next = new Position (x1, y1, dir);
			if (checkGridBounds(next)){
				if (!checkForDanger(new Edge(pos, next))){
					ArrayList<Position> path = dfs(next, depth - 1, explored);
					if (path != null && path.size() > 0){
						path.add(pos);
						return path;
					}
				}
			}
		}
		return null;
	}

	/**
	 * This begins the depth first search.
	 * @param start
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> idfs(Position start) throws ImpossibleException{
		int startIndex = (Constants.GRID_WIDTH - start.getX()) + (Constants.GRID_HEIGHT - start.getY());
		Position reverse = Utilities.getReverseDirection(start);
		if (startIndex < maxSearchDepth){
			for (int i = startIndex; i < maxSearchDepth; i++){
				ArrayList<Position> path = dfs(start, i, new HashMap<Position, Integer>());
				if (path != null && path.size() > 0){
					path.remove(path.size() - 1);
					return path;
				}
				ArrayList<Position> reverseDirPath = dfs(reverse, i, new HashMap<Position, Integer>());
				if (reverseDirPath != null && reverseDirPath.size() > 0){
					reverseDirPath.remove(reverseDirPath.size() - 1);
					return reverseDirPath;
				}
			}
		}
		return null;
	}

	/**
	 * Backwards depth first search. This starts at the goal and goes towards the hva's location.
	 * @param pos
	 * @param depth
	 * @param goal
	 * @param explored
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> backdfs(Position pos, int depth, Position goal, HashMap<Position, Integer> explored) throws ImpossibleException{
		if (pos.equals(goal)){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		if (depth == 0){
			return null;
		}
		if (explored.containsKey(pos) && explored.get(pos) >= depth){
			return null;
		}
		explored.put(pos, depth);
		ArrayList<Direction> branches = Utilities.getBackDfsBranches(pos, goal);
		for (Direction dir : branches){
			int x1 = pos.getX() + dir.getHoriz();
			int y1 = pos.getY() + dir.getVert();
			Position next = new Position(x1, y1, dir);
			if (checkGridBounds(next)){
				if (!checkForDanger(new Edge(pos, next))){
					ArrayList<Position> path = backdfs(next, depth - 1, goal, explored);

					if (path != null && path.size() > 0){
						path.add(pos);
						return path;	
					}
				}
			}
		}
		return null;
	}


	/**
	 * This kicks off the backwards DFS
	 * @param pos
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> ibackdfs(Position pos) throws ImpossibleException{
		Position hvaPos = hva.getPosition();
		int startIndex = Math.abs(hvaPos.getX() - pos.getX()) + Math.abs(hvaPos.getY() - pos.getY());
		Position reverse = Utilities.getReverseDirection(pos);
		if (startIndex < maxSearchDepth){
			for (int i = startIndex; i < maxSearchDepth; i++){
				ArrayList<Position> path = null;
				if (hva.getPath() != null && hva.getPath().size() > 0){
					path = backdfs(pos, i, hva.getPath().get(0), new HashMap<Position, Integer>());
				} else {
					path = backdfs(pos, i, hvaPos, new HashMap<Position, Integer>());
				}
				if (path != null && path.size() > 0){
					path.remove(path.size() - 1);
					return path;
				}
				//if failed, turn around
				if (hva.getPath() != null && hva.getPath().size() > 0){
					path = backdfs(reverse, i, hva.getPath().get(0), new HashMap<Position, Integer>());
				} else {
					path = backdfs(reverse, i, hvaPos, new HashMap<Position, Integer>());
				}
				if (path != null && path.size() > 0){
					path.remove(path.size() - 1);
					return path;
				}
			}
		}
		return null;
	}
	/**
	 * This is a DFS search for the HVA. It only uses the safe route.
	 * @param pos
	 * @param depth
	 * @param explored
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> safeHvaDfs(Position pos, int depth, HashMap<Position, Integer> explored) throws ImpossibleException{
		
		if (pos.getX() == Constants.GRID_WIDTH && pos.getY() == Constants.GRID_HEIGHT){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		
		if (suicide && pos.getX() == leader.getPosition().getX() && pos.getY() == leader.getPosition().getY()){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		
		if (depth == 0){
			System.out.println("Depth = 0!");
			return null;
			
		} 
		if (explored.containsKey(pos) && explored.get(pos) >= depth){
			System.out.println("Depth was too big!");
			return null;
		}
		explored.put(pos, depth);
		ArrayList<Direction> branches = Utilities.getSafeHvaDfsBranches(pos.getDir());
		for (Direction dir : branches){
			int x1 = pos.getX() + dir.getHoriz();
			int y1 = pos.getY() + dir.getVert();
			Position next = new Position(x1, y1, dir);
			if (checkGridBounds(next)){
				if (safeList.contains(new Edge(pos, next))){
					ArrayList<Position> path = safeHvaDfs(next, depth - 1, explored);
					if (path != null && path.size() > 0){
						path.add(pos);
						return path;	
					}
				}
			}
		}
		return null;
	}

	/**
	 * This kicks off the safe hva dfs
	 * @param start
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> iSafeHvaDfs(Position start) throws ImpossibleException{
		System.out.println("Running in safe mode");
		int startIndex = (Constants.GRID_WIDTH - start.getY()) + (Constants.GRID_HEIGHT - start.getY()); 
		Position reverse = Utilities.getReverseDirection(start);
		if (startIndex < maxSearchDepth){
			for (int i = startIndex; i < maxSearchDepth; i++){
				ArrayList<Position> path = safeHvaDfs(start, i, new HashMap<Position, Integer>());
				if (path != null && path.size() > 0){
					path.remove(path.size() - 1);
					return path;
				}
				ArrayList<Position> reverseDirPath = safeHvaDfs(reverse, i, new HashMap<Position, Integer>());
				if (reverseDirPath != null && reverseDirPath.size() > 0){
					reverseDirPath.remove(reverseDirPath.size() - 1);
					return reverseDirPath;
				}
			}
		}
		return null;
	}
	/**
	 * For the HVA, this only uses safe paths, going in a backwards order
	 * @param pos
	 * @param depth
	 * @param explored
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> regHvaDfs(Position pos, int depth, HashMap<Position, Integer> explored) throws ImpossibleException{
		if (pos.getX() == Constants.GRID_WIDTH && pos.getY() == Constants.GRID_HEIGHT){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		
		if (suicide && pos.getX() == leader.getPosition().getX() && pos.getY() == leader.getPosition().getY()){
			ArrayList<Position> posList = new ArrayList<Position>();
			posList.add(pos);
			return posList;
		}
		
		if (depth == 0){
			System.out.println("Depth = 0");
			return null;
		} 
		if (explored.containsKey(pos) && explored.get(pos) >= depth){
			System.out.println("Depth is too small!");
			return null;
		}
		explored.put(pos, depth);
		ArrayList<Direction> branches = Utilities.getDfsBranches(pos.getDir());
		for (Direction dir : branches){
			int x1 = pos.getX() + dir.getHoriz();
			int y1 = pos.getY() + dir.getVert();
			Position next = new Position(x1, y1, dir);
			if (checkGridBounds(next)){
				Edge edge = new Edge(pos, next);
				if (!checkForDanger(edge)){
					ArrayList<Position> path = regHvaDfs(next, depth - 1, explored);
					if (path != null && path.size() > 0 && safeList.contains(edge)){
						path.add(pos);
						return path;
					} 
					ArrayList<Position> posList = new ArrayList<Position>();
					posList.add(pos);
					return posList;
				}
			}
		}
		System.out.println("No Solution");
		return null;
	}
	/**
	 * This kicks off DFS, using safeHvaDfs for forward travel and regHvaDfs for going backwards.
	 * @param start
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> iHvaDfs(Position start) throws ImpossibleException{
		System.out.println("Running in regular mode");
		int startIndex = (Constants.GRID_WIDTH - start.getY()) + (Constants.GRID_HEIGHT - start.getY()); 
		Position reverse = Utilities.getReverseDirection(start);
		int searchDepth = maxSearchDepth;
		if (startIndex < searchDepth){
			for (int i = startIndex; i < searchDepth; i++){
				ArrayList<Position> path = null;
				path = safeHvaDfs(start, i*2, new HashMap<Position, Integer>());
				if (path == null || path.size() == 0){
					path = regHvaDfs(start, i, new HashMap<Position, Integer>());
					if ( path != null && path.size() >= 2){
						path.remove(path.size() - 1);
						path.remove(path.size() - 1);
					} else {
						path = null;
					}
				} else {
					path.remove(path.size() - 1);
					return path;
				}

				//turn around
				path = null;
				path = safeHvaDfs(reverse, i*2, new HashMap<Position, Integer>());
				if (path == null || path.size() == 0){
					System.out.println("Made a new path");
					path = regHvaDfs(reverse, i, new HashMap<Position, Integer>());
					if ( path != null && path.size() >= 2){
						System.out.println("Path size: "+path.size());
						return path;
					}
					System.out.println("New path didn't work");
					path = null;
				} else {
					path.remove(path.size() - 1);
					return path;
				}
			}
		}
		return null;
	}
	/**
	 * A simple interface for calling any type of search algorithm
	 * @param option
	 * @param pos
	 * @return
	 * @throws ImpossibleException
	 */
	public ArrayList<Position> callDfs(RoutingOption option, Position pos) throws ImpossibleException{
		switch (option){
		case DFS:
			return idfs(pos);
		case BACK_DFS:
			return ibackdfs(pos);
		case SAFE_HVA_DFS:
			return iSafeHvaDfs(pos);
		case REG_HVA_DFS:
			return iHvaDfs(pos);
		default:
			return null;
		}
	}

	/**
	 * Move the seeker one position
	 * @param seeker
	 * @throws ImpossibleException
	 */
	public void advance(Seeker seeker) throws ImpossibleException{ //TODO eventually this will be replaced by event driven seeker messages
		advanceCount++;
		if (advanceCount > 100){
			System.out.println("Too many advancings!");
			return;
		}
		if (seeker.getPath() != null && seeker.getPath().size() > 0){
			Position seekerTo = seeker.getPath().remove(seeker.getPath().size() - 1);
			Position currSeekerPos = seeker.getPosition();

			//Scan ahead of the seeker to check for hazards. Re-plan all seekers if one is found
			Edge edge = new Edge(currSeekerPos, seekerTo);
			boolean detectedObstacle = simulatedGrid.detectObstruction(edge);
			if (detectedObstacle){
				obstacleList.add(edge);
			} else {
				safeList.add(edge);
				animatedMap.addSafeEdge(edge);
			}
			if (!seeker.isHva() && detectedObstacle){
				seeker.setReplans(seeker.getReplans()+1);
				seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
				advance(seeker); //retry moving the seeker
				return;
			}
			try {
				simulatedGrid.move(currSeekerPos, seekerTo);
				animatedMap.smoothAnimate(seeker.getId(), currSeekerPos, seekerTo);
				seeker.setPosition(seekerTo);
				if (foundGoal(seeker.getPosition())){
					if (hva.getPath() != null && !hva.getPath().isEmpty() 
							&& foundGoal(hva.getPath().get(0))){
						seeker.setRoutingOption(RoutingOption.DONE);
					} else {
						if (!suicide){
							seeker.setRoutingOption(RoutingOption.BACK_DFS);
						}
					}
					goalFound = true;
					System.out.println("FOUND THE GOAL");
				}
				if (seeker.getRoutingOption() == RoutingOption.BACK_DFS && 
						seeker.getPosition() != null && seeker.getPosition().equals(hva.getPosition())){
					seeker.setRoutingOption(RoutingOption.DFS);
				}
			} catch (DestroyedException e){
				seeker.setRoutingOption(RoutingOption.DEAD);
				seeker.setPosition(new Position( -1, -1, Direction.UNKNOWN));
				seeker.setPath(null);
				animatedMap.setUMVDead(seeker.getId());
				
				if (suicide && seeker.getId() < 2){
					Seeker s = seekers.get(seeker.getId()+1);
					s.setRoutingOption(RoutingOption.DFS);
					s.setPath(callDfs(s.getRoutingOption(), s.getPosition()));
					leader = s;
				} 
			}
		}
		if (seeker.getPath() == null || seeker.getPath().size() == 0){
			seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
			seeker.setReplans(seeker.getReplans()+1);
			//replan all seekers if the hva needs to replan
			if (seeker.isHva() && !suicide){
				System.out.println("HVA causes all seekers to replan.");
				if (seeker.getPath() == null){
					System.out.println("Path is null");
				}
				for (Seeker s : seekers){
					s.setPath(callDfs(s.getRoutingOption(), s.getPosition()));
				}
			}
		}
	}

	private boolean foundGoal(Position pos){
		if (pos == null){
			return false;
		}
		return (pos.getX() == Constants.GRID_WIDTH && pos.getY() == Constants.GRID_HEIGHT);
	}

	private void initSeekers(){
		System.out.println("Begin seeker init");
		if (animatedMap == null){
			animatedMap = new AnimatedMap(this, false);
			animatedMap.draw();
		}		

		seekers.clear();
		//set the initial state of the seekers;
		for (int seekerNum = 0; seekerNum < Constants.NUMBER_SEEKERS; seekerNum++){
			Seeker seeker = new Seeker(seekerNum, false);
			seeker.setRoutingOption(RoutingOption.DFS);
			seekers.add(seeker);
			System.out.println("Creating a seeker: "+seekerNum);
		}
		if (suicide){
			seekers.get(1).setRoutingOption(RoutingOption.REG_HVA_DFS);
			seekers.get(2).setRoutingOption(RoutingOption.REG_HVA_DFS);
			System.out.println("Seekers sent to follow the leader.");
			leader = seekers.get(0);
		}
		hva = new Seeker(-1, true);
		hva.setRoutingOption(RoutingOption.REG_HVA_DFS);

		//reset state variables
		obstacleList.clear();
		safeList.clear();
		goalFound = false;
		seekerSteps = 0;
		hvaSteps = 0;

	}

	public void handlePositionUpdate(int seekerId, Position pos) throws ImpossibleException{
		System.out.println("Updating seeker: "+(seekerId+1));
		if (seekerId == 0){
			seeker1Steps++;
		}
		//now it is time to move the 2nd seeker for the first time.
		if (seeker1Steps > 1 && seekers.get(1).getPosition() == null){ 
			if (pathOpen(seekers.get(1), new Position(0,0))){
				ms.sendMessage(1, 0, 0);
			}
			//now it is time to move the 3rd seeker for the first time.
		} else if (seeker1Steps > Constants.THIRD_DELAY && seekers.get(2).getPosition() == null){ 
			if (pathOpen(seekers.get(2), new Position(0,0))){
				ms.sendMessage(2, 0, 0);
			}
		} 
		//now it is time to move the hva for the first time.
		if (seekerId == 2 && hva.getPosition() == null){
			if (pathOpen(seekers.get(-1), new Position(0,0))){
				ms.sendMessage(-1, 0, 0);
			}
		}

		Seeker seeker = hva;
		if (seekerId > -1){
			seeker = seekers.get(seekerId);
		}
		seeker.setAlive(true); //just in case we thought the seeker died

		//mark edge as safe
		Position old = seeker.getPosition();
		if (old != null){
			Edge safe = new Edge(old, pos);
			safeList.add(safe);
			animatedMap.addSafeEdge(safe);
		}

		//update current position
		seeker.setPosition(pos);

		//TODO determine when to go off the map, right now, we always end
		if (pos.getX() == 5 && pos.getX() == 5){
			if (!suicide && !seeker.isHva() && hva.getPosition().getX()+hva.getPosition().getX() < 5){
				seeker.setRoutingOption(RoutingOption.BACK_DFS);
				seeker.setPath(null);
				moveSeeker(seeker);
			} else {
				ms.sendMessage(seeker.getId(), endPosition, endPosition);
				endPosition = endPosition - 1;
				seeker.setRoutingOption(RoutingOption.DONE);
			}
		} else {
			moveSeeker(seeker);
		}
		animatedMap.smoothAnimate(seeker.getId(), old, pos);
	}

	public void handleDeadSeeker(int seekerId) throws ImpossibleException{
		Seeker seeker = hva;
		if (seekerId > -1){
			seeker = seekers.get(seekerId);
		}
		seeker.setAlive(false);
		animatedMap.setUMVDead(seekerId);
		if (suicide && seekerId < 2){
			Seeker s = seekers.get(seekerId+1);
			s.setRoutingOption(RoutingOption.DFS);
			s.setPath(callDfs(s.getRoutingOption(), s.getPosition()));
			leader = s;
		} 
		
		//check if there is a live seeker, else hva should change strategy
		for (Seeker s : seekers){
			if (s.isAlive()){
				return;
			}
		}
		
		hva.setRoutingOption(RoutingOption.DFS);

	}

	public void addVisualIed(int seekerId, Edge edge1, Edge edge2, Edge edge3, Edge edge4) throws ImpossibleException {
		obstacleList.add(edge1);
		obstacleList.add(edge2);
		obstacleList.add(edge3);
		obstacleList.add(edge4);
		Seeker seeker = hva;
		if (seekerId > -1){
			seeker = seekers.get(seekerId);
		}

		seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
		moveSeeker(seeker);
		updateIedMap();
	}

	public void addSonarIed(int seekerId, Edge edge) throws ImpossibleException{
		if (sonarMap.containsKey(edge)){
			Integer n = sonarMap.get(edge);
			sonarMap.put(edge, n+1);
		} else {
			sonarMap.put(edge, 1);
		}
		Seeker seeker = hva;
		if (seekerId > -1){
			seeker = seekers.get(seekerId);
		}
		seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
		moveSeeker(seeker);

		updateIedMap();
	}

	public void addObstacle(int seekerId, Edge edge) throws ImpossibleException{
		obstacleList.add(edge);

		Seeker seeker = hva;
		if (seekerId > -1){
			seeker = seekers.get(seekerId);
		}

		seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
		moveSeeker(seeker);
		animatedMap.addObstacle(edge);
	}

	public void moveSeeker(Seeker seeker) throws ImpossibleException{
		if (seeker.getPath() == null || seeker.getPath().size() == 0){
			seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
			seeker.setReplans(seeker.getReplans()+1);
			//replan all seekers if the hva needs to replan
			if (seeker.isHva()){
				System.out.println("HVA causes all seekers to replan.");
				if (seeker.getPath() == null){
					System.out.println("Path is null");
				}
				for (Seeker s : seekers){
					s.setPath(callDfs(s.getRoutingOption(), s.getPosition()));
				}
			}
		}

		//tell it where to go next
		if (seeker.getPath() != null && seeker.getPath().size() > 0){
			Position seekerTo = seeker.getPath().get(seeker.getPath().size() - 1);
			//check if path contains a known danger, replan seeker
			if (checkForDanger(new Edge(seeker.getPosition(), seekerTo))){
				seeker.setPath(callDfs(seeker.getRoutingOption(), seeker.getPosition()));
				seeker.setReplans(seeker.getReplans()+1);
				moveSeeker(seeker);
				return;
			}
			if (pathOpen(seeker, seekerTo)){ //only move if it is safe to move.
				ms.sendMessage(seeker.getId(), seekerTo.getX(),  seekerTo.getY());
				seeker.getPath().remove(seeker.getPath().size() - 1);
			} else {
				seeker.setBlocked(true); //TODO, if we're not the hva, see if we can change paths?
			}
		}

		//check all blocked seekers and see if they can now move
		for (Seeker blockedSeeker : seekers){
			if (blockedSeeker.isBlocked()){
				Position seekerTo = blockedSeeker.getPath().get(blockedSeeker.getPath().size() - 1);
				if (pathOpen(blockedSeeker, seekerTo)){ //only move if it is safe to move.
					ms.sendMessage(blockedSeeker.getId(), seekerTo.getX(),  seekerTo.getY());
					blockedSeeker.getPath().remove(blockedSeeker.getPath().size() - 1);
				}
			}
		}

		if (hva.isBlocked()){
			Position seekerTo = hva.getPath().get(hva.getPath().size() - 1);
			if (pathOpen(hva, seekerTo)){ //only move if it is safe to move.
				ms.sendMessage(hva.getId(), seekerTo.getX(),  seekerTo.getY());
				hva.getPath().remove(hva.getPath().size() - 1);
			}
		}
	}

	private boolean checkForDanger(Edge edge){
		return checkForIed(edge) && !obstacleList.contains(edge);
	}
	
	private boolean checkForIed(Edge edge){

		if (sonarMap.size() != 0){
			Integer total = 0;
			for (Integer i : sonarMap.values()){
				total = total + i;
			}
			double average = total*1.0 / sonarMap.size();

			Integer hits = sonarMap.get(edge);
			if (hits != null && hits > 5 && hits >= average && visualIedList.contains(edge)){
				return true;
			}
		}
		return false;
	}

	private void updateIedMap(){

		synchronized(this){
			for (Edge e : sonarMap.keySet()){
				if (checkForIed(e)){
					animatedMap.addIed(e);
				}
			}
		}
	}


private boolean checkGridBounds(Position pos){

	if (pos.getX() < 0 || pos.getX() > Constants.GRID_WIDTH
			|| pos.getY() < 0 || pos.getY() > Constants.GRID_HEIGHT){
		return false;
	}
	return true;
}
/**
 * This starts the simulator
 * @param random
 * @throws ImpossibleException
 */
public void run(boolean random) throws ImpossibleException{

	started = true;

	System.out.println("Started");
	if (animatedMap == null){
		animatedMap = new AnimatedMap(this, true);
		animatedMap.draw();
	}		

	int sumOfDistances = 0;
	int sumOfDistancesSquare = 0;
	int maxDistance = 0;
	int sumOfDestroyed = 0;
	int sumOfDestroyedSquare = 0;
	int maxDestroyed = 0;
	int sumOfFailed = 0;

	for (int trial = 0; trial < Constants.MAX_MAZES; trial++){

		seekers.clear();
		//set the initial state of the seekers;
		for (int seekerNum = 0; seekerNum < Constants.NUMBER_SEEKERS; seekerNum++){
			Seeker seeker = new Seeker(seekerNum, false);
			seeker.setRoutingOption(RoutingOption.DFS);
			seeker.setPosition(new Position(0, 0));
			seekers.add(seeker);
		}
		
		if (suicide){
			seekers.get(1).setRoutingOption(RoutingOption.REG_HVA_DFS);
			seekers.get(2).setRoutingOption(RoutingOption.REG_HVA_DFS);
			System.out.println("Seekers sent to follow the leader.");
			leader = seekers.get(0);
		}
		
		hva = new Seeker(-1, true);
		hva.setRoutingOption(RoutingOption.REG_HVA_DFS);
		hva.setPosition(new Position(0,0));

		//reset state variables
		obstacleList.clear();
		safeList.clear();
		goalFound = false;
		seekerSteps = 0;
		hvaSteps = 0;

		simulatedGrid = new SimulatedGridReading(animatedMap);
		if (random){
			simulatedGrid.setup();
		} else {
			simulatedGrid.setup(animatedMap.getObstacles(), animatedMap.getIeds());
		}

		//Initialize seekers
		Seeker seeker1 = seekers.get(0);
		Seeker seeker2 = seekers.get(1);
		Seeker seeker3 = seekers.get(2);

		//Plan for the first two seekers
		if (seeker1 != null){
			Position pos = seeker1.getPosition();
			pos.setDir(Direction.EAST);
			seeker1.setPosition(pos);
			seeker1.setPath(callDfs(seeker1.getRoutingOption(), seeker1.getPosition()));
		}
		if (seeker2 != null){
			Position pos = seeker2.getPosition();
			pos.setDir(Direction.NORTH);
			seeker2.setPosition(pos);
			seeker2.setPath(callDfs(seeker2.getRoutingOption(), seeker2.getPosition()));
		}

		//Run only the first two seekers until the time when the third seeker must move
		for (int i = 0; i < Constants.THIRD_DELAY; i++){
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				//zzz
			}
			if (seeker1 != null){
				advanceCount = 0;
				advance(seeker1);
			}
			if (seeker2 != null){
				advanceCount = 0;
				advance(seeker2);
			}
		}

		//Set the direction to initially search the same as the least deviated seeker
		Direction searchDir = Direction.UNKNOWN;
		if (seeker1 != null && seeker2 != null){
			if (seeker1.getRoutingOption()!= RoutingOption.DEAD
					&& seeker1.getReplans() <= seeker2.getReplans()){
				searchDir = Direction.EAST;
			} else if (seeker2.getRoutingOption()!= RoutingOption.DEAD
					& seeker2.getReplans() <= seeker1.getReplans()){
				searchDir = Direction.NORTH;
			} else if (seeker1.getReplans() <= seeker2.getReplans()){
				searchDir = Direction.EAST;
			} else {
				searchDir = Direction.NORTH;
			}
		}

		//initialize seeker 3 path
		Position pos = seeker3.getPosition();
		pos.setDir(searchDir);
		seeker3.setPosition(pos);
		hva.setPosition(pos);
		seeker3.setPath(callDfs(seeker3.getRoutingOption(), seeker3.getPosition()));
		advanceCount = 0;
		advance(seeker3);

		//Move all seekers until the HVA reaches the goal
		int breaker = 0;
		while (!foundGoal(hva.getPosition())){
			try {
				Thread.sleep(1000);
			} catch (InterruptedException e) {
				//zzz
			}
			breaker++;
			if (breaker > 100){
				System.out.println("Ran for too long.");
				break;
			} for (Seeker seeker : seekers){
				advanceCount = 0;
				advance(seeker);
			}
			seekerSteps++;
			//if (1.0*hvaSteps/seekerSteps < 1.0*Constants.HVA_SPEED/Constants.SEEKER_SPEED){
			advanceCount = 0;
			advance(hva);
			hvaSteps++;
			//}
			boolean aliveSeeker = false;
			for (Seeker seeker : seekers){
				if (!seeker.isHva() && seeker.getRoutingOption() != RoutingOption.DEAD){
					aliveSeeker = true;
				}
			}
			if (!aliveSeeker){
				if (!goalFound){
					//if there are no seekers and no path to the goal, just take our chances
					hva.setRoutingOption(RoutingOption.DFS);
				} else {
					hva.setRoutingOption(RoutingOption.SAFE_HVA_DFS);
				}
			}
			if (hva.getRoutingOption() == RoutingOption.DEAD){
				sumOfFailed++;
				break;
			}
		}
		//Record relevant parameters for run
		sumOfDistances = sumOfDistances + hvaSteps;
		sumOfDistancesSquare = sumOfDistancesSquare + hvaSteps*hvaSteps;
		maxDistance = Math.max(maxDistance, hvaSteps);
		int numDestroyed = 0;
		for (Seeker seeker : seekers){
			if (seeker.getRoutingOption() == RoutingOption.DEAD){
				sumOfDestroyed++;
				numDestroyed++;
			}
		}
		sumOfDestroyedSquare = sumOfDestroyedSquare + numDestroyed*numDestroyed;
		maxDestroyed = Math.max(maxDestroyed, numDestroyed);

		animatedMap.clearAll();
	}

	double averageDistance = 1.0*(sumOfDistances)/Constants.MAX_MAZES;
	double distanceVar = 1.0*(sumOfDistancesSquare)/Constants.MAX_MAZES - averageDistance*averageDistance;
	double distanceStdDev = Math.sqrt(distanceVar);
	double averageDestroyed = 1.0*(sumOfDestroyed)/Constants.MAX_MAZES;
	double destroyedVar = 1.0*(sumOfDestroyedSquare)/Constants.MAX_MAZES - averageDestroyed*averageDestroyed;
	double destroyedStdDev = Math.sqrt(destroyedVar);

	System.out.println("Average Distance:          " + averageDistance + "+/-" + distanceStdDev);
	System.out.println("Max Distance:              " + maxDistance);
	System.out.println("Average Seekers Destroyed: " + averageDestroyed + "+/-" + destroyedStdDev);
	System.out.println("Max Destroyed:             " + maxDestroyed);
	System.out.println("Average Failed:            " +  1.0*(sumOfFailed)/Constants.MAX_MAZES);

	started = false;
	animatedMap.resetStartButton();
}

public void start(boolean random){
	try {
		run(random);
	} catch (ImpossibleException e) {
		e.printStackTrace();
	}
}

private boolean pathOpen(Seeker seeker, Position pos){
	try{
		for (Seeker s : seekers){
			if (s.isAlive()){
				Position goal = s.getPath().get(s.getPath().size() - 1);
				if (pos.equals(s.getPath()) || pos.equals(goal)){
					return false;
				}
			}
		}
		Position goal = hva.getPath().get(hva.getPath().size() - 1);
		if (pos.equals(hva.getPath()) || pos.equals(goal)){
			seeker.setRoutingOption(RoutingOption.DFS);
			seeker.setPath(null);
			moveSeeker(seeker);
			return false;
		}
	} catch (Exception e){ //null pointers and such
		return true;
	}
	return true;
}
public static void main(String[] args){

	System.out.println("Started!");
	RoutingAlgorithm r = new RoutingAlgorithm(false, false);
}
}












